/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This is an effort to test if the Occupancygrid can be used for SLAMing
*/ 

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "CScanMatchInterface.h" ///< Scanmatch interface
#include "CAHistogramECorrelationScanMatch.h" ///< Scanmatch 
#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "EnvironmentMeasurement2D.h"

/// For reading the data from file
int readScanLine181(FILE *f,ScanMatch::scan &meas, ScanMatch::pose &p ){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f %f %f",&p.x,&p.y,&p.a); ///< Pose
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f %f",&meas.r[i],&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						//fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

void scan2absoluteXY( ScanMatch::scan &s, float *x, float *y, ScanMatch::pose &pose){
	int i;
	if(x==NULL || y==NULL) return;
	for(i=0;i<s.N;i++){
		x[i]=s.r[i]*cos(s.a[i]+pose.a)+pose.x;
		y[i]=s.r[i]*sin(s.a[i]+pose.a)+pose.y;
	}
}

void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			setRGBpix(rgb,i,j, Nx, r*data[Nx*j+i],g*data[Nx*j+i],b*data[Nx*j+i]);
		}
	}	
}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float dy = sin(angle);
		float dx = cos(angle);
		float rr=0;
		int i=0;
		uint x,y;
		while(rr<length){
				x = pix + i*dx;
				y = piy - i*dy;
				RGB[3*Nx*y+3*x] = r;
				RGB[3*Nx*y+3*x+1] = g;
				RGB[3*Nx*y+3*x+2] = b;
				i++;
				rr+=resolution;
		}
}


int filterScan(ScanMatch::scan &m, Grid::TScan &g){
		int bad=0;
		float dist_th=1.0;
		float dr_plus,dr_minus;
		
		for(int i=1;i<m.N-1;i++){
				dr_plus = fabs(m.r[i]-m.r[i+1]);
				dr_minus = fabs(m.r[i]-m.r[i-1]);
				if(dr_plus>dist_th && dr_minus>dist_th) g.r[i] = -1.0; 
		}
		for(int i=0;i<m.N;i++){
				g.a[i] = m.a[i];
				if(g.r[i] == -1){ g.r[i]=0; bad++;} 
				else g.r[i] = m.r[i];
				if(g.r[i] > 35){g.r[i] = 0; bad++;}///Filter out long distances
		}
		return bad;
}



int main(void){
		float grid_size = 135.0; ///< size of the local grid
		
		//TOccupancyGrid O(grid_size,grid_size,0.04,Grid::TCoord2D(0,0));
		TOccupancyGrid O(grid_size,grid_size,0.1,Grid::TCoord2D(0,0));
		TOccupancyGrid O_10(grid_size,grid_size,0.1,Grid::TCoord2D(0,0));
		TOccupancyGrid O_15(grid_size,grid_size,0.15,Grid::TCoord2D(0,0));
		
		O.setGridToValue(0.5);
		O_10.setGridToValue(0.5);
		O_15.setGridToValue(0.5);
	
		CEnvironmentMeasurement2D env_meas;

		grid_t *dataPtr;
		int Nx,Ny;	
		unsigned char *rgb;
		
		FILE *fenv_meas = fopen("environment_measurement2D_slam.txt","wt");
		if(fenv_meas==NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		FILE *fenv_meas_10 = fopen("environment_measurement2D_slam_10cm.txt","wt");
		if(fenv_meas==NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
		//FILE *log = fopen("../../../data/j2b2_measurement.txt","rt"); ///< data file
		//FILE *log = fopen("j2b2_measurement_labrat.txt","rt"); ///< data file
		FILE *log = fopen("bbdt_measurement.txt","rt"); ///< data file
		if(log == NULL){
				fprintf(stderr,"Unable to open the log file\n");
				exit(1);
		}
	
		///Scanmatch stuff
		CScanMatchInterface *gmatcher; ///< This is for global	
		gmatcher = new CAHistogramECorrelationScanMatch(
																						.6,   // Search area in meters 
																						.6);     // Search angle in radians
		
		ScanMatch::pose pos,odo_cur,odo_ref,est;   ///< The estimated position
		ScanMatch::scan meas,ref,meas0;  ///< The Reference scan 
		meas.allocate(181);
		ref.allocate(181);
		meas0.allocate(181);		
		///Initialize grid as empty
 		
		O.getDim(Nx,Ny);
		
		///Initialize Image display
		rgb = (unsigned char *) malloc(3*Nx*Ny);
		addImageWindow(Nx, Ny);
		glutImageGUI_Start();
		
		Grid::TScan s,virt; ///< for occupancy grid
		s.alloc(181);
		virt.alloc(181);
		readScanLine181(log,meas0,odo_cur);
	
		odo_cur.set(0,0,0);
		O.setScanFillProb(odo_cur.x, odo_cur.y ,odo_cur.a, *((Grid::TScan *) &meas0),0.01,0.99);
		O.fillRGB(rgb,1.0/255.0);
		displayImage(0, rgb,Ny,Nx);
		
		long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
		float fms;
		ownTime_GetTimeOfDay(&ss0,&us0);
		int cnt=0;
		odo_ref = odo_cur;
    fprintf(stderr,"ref.N=%d meas.N=%d  \n",ref.N,meas.N);
		
		float updatesincedist=1000;
		
		bool origin_updated = false;
		
		std::vector< std::vector<float> > scans; ///All scans for previous map
		std::vector< ScanMatch::pose > poses;    ///All poses for previous map
		std::vector<float> tmp_scan;
		
		 //PLOT ODOMETRY
		while(!feof(log)){
				readScanLine181(log,meas,odo_cur);
				poses.push_back(odo_cur);
				meas.filt(15.0);
				O.setScanFillProb(odo_cur.x, odo_cur.y ,odo_cur.a,*((Grid::TScan *) &meas),0.3,0.9);
				//fprintf(stderr,"(%.2f %.2f %.2f)",odo_cur.x, odo_cur.y ,odo_cur.a);
				usleep(1000*10);
				if(cnt%100==0){
						O.fillRGB(rgb,1.0/255.0);
						for(int i=0;i<poses.size();i++){
								Grid::TPixel pix = O.toPixel(Grid::TCoord2D(poses[i].x,poses[i].y));
								setRGBpix(rgb,pix.x,pix.y, Nx, 255,0, 0 );
						}
						
						displayImage(0, rgb,Ny,Nx);
				}
				cnt++;
		}
		
		while(!feof(log)){
				readScanLine181(log,meas,odo_cur);
				tmp_scan.clear();
				for(int i=0;i<meas.N;i++){
						tmp_scan.push_back(meas.r[i]);
				}
				scans.push_back(tmp_scan);
			
				meas.filt(25.0);
				////////////////////////////////////////////////////////////////////////////
				/// The SLAM
				////////////////////////////////////////////////////////////////////////////
				
				est = est.calculateDifferentialSensorMovement(odo_ref,odo_cur, 0);
				odo_ref = odo_cur;
				pos = pos.integrateDifferential(est);
				O.getScan(pos.x,pos.y, (pos.a-M_PI/2.0), 181, M_PI/180.0, (grid_t) 0.7, virt); ///<getscan
				int badmeas =0;
				for(int i=0;i<meas.N;i++){
						ref.a[i]=virt.a[i]-M_PI/2.0;
						ref.r[i]=virt.r[i]; 
						if (virt.r[i]<0){
								ref.r[i]=0;
								badmeas++;
						}
				}
				
				gmatcher->setCurrentPose(pos);	
				
				//if(badmeas > 5) {
				//		fprintf(stderr,"Bad Measurement =%d\n",badmeas);
			//	}else{
						gmatcher->setReferenceScan(ref,pos); 
						gmatcher->update(meas,pos);    ///update
						gmatcher->getCurrentPose(pos); ///< Get the estimate			
				//}
				int iCorrect,iWrong;
				float corr = O.getMeasurementLikelihood(pos.x, pos.y, pos.a,*((Grid::TScan *) &meas), iCorrect, iWrong);
				fprintf(stderr,"Corr = %.5f Right=%d Wrong=%d\n",corr,iCorrect,iWrong);
																//ScanMatch::pose pos, ScanMatch::scan meas){
				poses.push_back(pos); ///Save pose for this scan
				
				///Set measurement to map
				filterScan(meas,s);
				O.setScanFillProb(pos.x, pos.y ,pos.a,s,0.3,0.9);
				O_10.setScanFillProb(pos.x, pos.y ,pos.a,s,0.1,0.9);
				O_15.setScanFillProb(pos.x, pos.y ,pos.a,s,0.1,0.9);
				
				////////////////////////////////////////////////////////////////////////////
				/// Resetting the map
				////////////////////////////////////////////////////////////////////////////
				float toPos = sqrt(pos.x*pos.x+pos.y*pos.y);
				float toEdge;
				toEdge = (grid_size/2.0)-toPos;			
				if(toEdge < 10.0 || feof(log)){
						env_meas.setOccupancyGrid(&O, 0.8); ///< set grid to 2D measurement 
						env_meas.frame.setFrame(pos.x,pos.y,pos.a);		
						env_meas.frame.setError(pos.x*0.1,pos.y*0.1,pos.a*0.1);		
						
						fprintf(stderr,"Saving Original map\n");
						env_meas.fprint(fenv_meas);        ///< Save to file
						fflush(fenv_meas);                 ///< flush
						
						fprintf(stderr,"Saving map with 10cm resolution\n");
						env_meas.setOccupancyGrid(&O_10, 0.8); ///< set grid to 2D measurement 
						env_meas.fprint(fenv_meas_10);        ///< Save to file
						fflush(fenv_meas_10);                 ///< flush
												
						O.setGridToValue(0.5); ///< Clear map
						O_10.setGridToValue(0.5); ///< Clear map
						
						pos.set(0,0,0); ///< initialize pose
						O.setScanFillProb(0, 0 ,0, *((Grid::TScan *) &meas),0.01,0.99); ///< set last measurement to map
						O_10.setScanFillProb(0, 0 ,0, *((Grid::TScan *) &meas),0.01,0.99); ///< set last measurement to map
						origin_updated = false;
						
						for(unsigned int i=1;i<scans.size()-2;i+=1){
								for(int j=0;j<meas.N;j++){
										meas.r[j]=scans[scans.size()-i-1][j]; ///< starting from the last measurement
								}
								//fprintf(stderr,"Ind=%d scans[%d][%d], N=%d ",scans.size()-i-1,scans.size(),scans[0].size(),meas.N);
								meas.filt(10.0);
								est = est.calculateDifferentialSensorMovement(poses[scans.size()-1],
																																		poses[scans.size()-1-i], 0);
								//odo_ref = odo_cur;
								//pos = pos.integrateDifferential(est);
								//fprintf(stderr,"Setting Measurements to the map!\n");
								O.setScanFillProb(est.x, est.y ,est.a, *((Grid::TScan *) &meas),0.3,0.9);
								//O.fillRGB(rgb,1.0/255.0);
								//displayImage(0, rgb,Ny,Nx);
								//usleep(1000*1000);
						}
						
						scans.clear();
						poses.clear();
				}
				
				////////////////////////////////////////////////////////////////////////////
				/// Visualisation
				////////////////////////////////////////////////////////////////////////////
				/// SLAM

				if(cnt%10==0){
					//fprintf(stdout,"Drawing");
					O.fillRGB(rgb,1.0/255.0);
					displayImage(0, rgb,Ny,Nx);
				}
				////////////////////////////////////////////////////////////////////////////
				/// The end
				////////////////////////////////////////////////////////////////////////////
				
				cnt++;
		}
		fclose(fenv_meas);
		fclose(fenv_meas_10);
		
		ownTime_GetTimeOfDay(&ss2,&us2);
		sec = ss2-ss0;us = us2-us0; ms = sec*1000+us/1000;
		fms=(float)sec*1000.0+(float)us/1000.0;
		fprintf(stderr,"TOTAL::(%.4f) = (%.4f)/scan \n",fms,fms/cnt);
		
		fprintf(stderr,"The End - press ctrl C to stop\n");
		while(1) usleep(1000*1000);
		
		return 0;
}